
// import oimo.collision.geometry.*;
// import oimo.collision.narrowphase.*;
// import oimo.collision.narrowphase.detector.gjkepa.*;
// import oimo.common.*;
// import oimo.m.*;

import { Detector } from "./Detector";
import { DetectorResult } from "../DetectorResult";
import { Geometry } from "../../geometry/Geometry";
import { Transform } from "../../../common/Transform";
import { CachedDetectorData } from "./CachedDetectorData";
import { Setting } from "../../../common/Setting";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { GjkEpa } from "./gjkepa/GjkEpa";
import { ConvexGeometry } from "../../geometry/ConvexGeometry";
import { GjkEpaResultState } from "./gjkepa/GjkEpaResultState";

/**
 * General convex collision detector using GJK/EPA
 */

export class GjkEpaDetector extends Detector {
	/**
	 * Default constructor.
	 */
	constructor() {
		super(false);
	}

	public detectImpl(result: DetectorResult, geom1: Geometry, geom2: Geometry, tf1: Transform, tf2: Transform, cachedData: CachedDetectorData): void {
		var gjkEpa: GjkEpa = GjkEpa.getInstance();
		var g1: ConvexGeometry = geom1 as ConvexGeometry;
		var g2: ConvexGeometry = geom2 as ConvexGeometry;
		var status: number = gjkEpa.computeClosestPoints(g1, g2, tf1, tf2, Setting.enableGJKCaching ? cachedData : null);
		result.incremental = true;

		if (status != GjkEpaResultState.SUCCEEDED) {
			console.error("GJK/EPA failed: status=" + status);
			return;
		}

		var margin1: number = g1._gjkMargin;
		var margin2: number = g2._gjkMargin;

		if (gjkEpa.distance > margin1 + margin2) { // geometries are separating
			return;
		}
		var pos1: Vec3 = gjkEpa.closestPoint1.clone();
		var pos2: Vec3 = gjkEpa.closestPoint2.clone();

		var normal: Vec3 = new Vec3();
		M.vec3_sub(normal, pos1, pos2);

		if (M.vec3_dot(normal, normal) == 0) {
			return; // core geometries are just touching
		}
		if (gjkEpa.distance < 0) {
			normal.negateEq();
		}
		normal.normalize();
		this.setNormal(result, normal);

		// move the closest points to the surface of the geometries
		M.vec3_addRhsScaledEQ(pos1, pos1, normal, -g1._gjkMargin);
		M.vec3_addRhsScaledEQ(pos2, pos2, normal, g2._gjkMargin);
		this.addPoint(result, pos1, pos2, g1._gjkMargin + g2._gjkMargin - gjkEpa.distance, 0);
	}

}
